// This example demonstrates CmdMessenger's callback  & attach methods
// For Arduino Uno and Arduino Duemilanove board (may work with other)

// Download these into your Sketches/libraries/ folder...

// CmdMessenger library available from https://github.com/dreamcat4/cmdmessenger
#include <CmdMessenger.h>

// Base64 library available from https://github.com/adamvr/arduino-base64
#include <Base64.h>

// Streaming4 library available from http://arduiniana.org/libraries/streaming/
#include <Streaming.h>

#include <AFMotor.h>

enum
{
  DISABLED  = 0,
  MANUAL    = 1,
  PC        = 2,
};

boolean trackingState = PC;

int MICRO_STEP_LIMIT = 48;

AF_Stepper motor(MICRO_STEP_LIMIT, 1);

// Mustnt conflict / collide with our message payload data. Fine if we use base64 library ^^ above
char field_separator = ',';
char command_separator = ';';

// Attach a new CmdMessenger object to the default Serial port
CmdMessenger cmdMessenger = CmdMessenger(Serial, field_separator, command_separator);


// ------------------ C M D  L I S T I N G ( T X / R X ) ---------------------

// We can define up to a default of 50 cmds total, including both directions (send + recieve)
// and including also the first 4 default command codes for the generic error handling.
// If you run out of message slots, then just increase the value of MAXCALLBACKS in CmdMessenger.h

// Commands we send from the Arduino to be received on the PC
enum
{
  kCOMM_ERROR    = 000, // Lets Arduino report serial port comm error back to the PC (only works for some comm errors)
  kACK           = 001, // Arduino acknowledges cmd was received
  kARDUINO_READY = 002, // After opening the comm port, send this cmd 02 from PC to check arduino is ready
  kERR           = 003, // Arduino reports badly formatted cmd, or cmd not recognised

  // Now we can define many more 'send' commands, coming from the arduino -> the PC, eg
  // kICE_CREAM_READY,
  // kICE_CREAM_PRICE,
  // For the above commands, we just call cmdMessenger.sendCmd() anywhere we want in our Arduino program.

  kSEND_CMDS_END, // Mustnt delete this line
};

// Commands we send from the PC and want to recieve on the Arduino.
// We must define a callback function in our Arduino program for each entry in the list below vv.
// They start at the address kSEND_CMDS_END defined ^^ above as 004
messengerCallbackFunction messengerCallbacks[] = 
{
  turn,            // 004
  releaseMotor,    // 005
  setState,        // 006
  NULL
};
// Its also possible (above ^^) to implement some symetric commands, when both the Arduino and
// PC / host are using each other's same command numbers. However we recommend only to do this if you
// really have the exact same messages going in both directions. Then specify the integers (with '=')

/* NES INITIALIZATION */
int latch = 16; // set the latch pin
int clock = 14; // set the clock pin
int datin = 18;// set the data in pin
byte controller_data = 0;
int ledpin = 11;

int manualSpeed = 60;
int manualSteps = 1;

/*END NES INITIALIZATION*/

const int bufferSize = 20;

// ------------------ C A L L B A C K  M E T H O D S -------------------------

void turn() //func[4],direction[0=forward, 1=backward],speed,steps;
{
  if(trackingState != PC)
  { //Just eat the buffer
    char buf[bufferSize] = { '\0' };
    cmdMessenger.copyString(buf, bufferSize);
    return;
  }
  
  int turnDirection = FORWARD;
  int turnSpeed = 100;
  int steps = 20;

  //cmdMessenger.sendCmd(kACK,"turning");
  while ( cmdMessenger.available() )
  {
    char buf[bufferSize] = { '\0' };
    cmdMessenger.copyString(buf, bufferSize);
    turnDirection = atoi(buf);
 
    if(turnDirection == true)
      turnDirection = FORWARD;
    else
      turnDirection = BACKWARD;
      
    cmdMessenger.copyString(buf, bufferSize);
    turnSpeed = atoi(buf);

    cmdMessenger.copyString(buf, bufferSize);
    steps = atoi(buf);

    motor.setSpeed(turnSpeed);

    if(steps < MICRO_STEP_LIMIT)
    {
      motor.step(steps, turnDirection, MICROSTEP);
    }
    else
    {
      motor.step(steps, turnDirection, DOUBLE);
    }
  }
}

void releaseMotor()
{
  cmdMessenger.sendCmd(kACK,"M.rel");
  motor.release();
}

// ------------------ D E F A U L T  C A L L B A C K S -----------------------

void arduino_ready()
{
  // In response to ping. We just send a throw-away Acknowledgement to say "im alive"
  cmdMessenger.sendCmd(kACK,"Ready");
}

void unknownCmd()
{
  // Default response for unknown commands and corrupt messages
  cmdMessenger.sendCmd(kERR,"Unknown");
}

// ------------------ E N D  C A L L B A C K  M E T H O D S ------------------



// ------------------ S E T U P ----------------------------------------------

void attach_callbacks(messengerCallbackFunction* callbacks)
{
  int i = 0;
  int offset = kSEND_CMDS_END;
  while(callbacks[i])
  {
    cmdMessenger.attach(offset+i, callbacks[i]);
    i++;
  }
}

void setup() 
{
  // Listen on serial connection for messages from the pc
  // Serial.begin(57600);  // Arduino Duemilanove, FTDI Serial
  Serial.begin(9600); // Arduino Uno, Mega, with AT8u2 USB

/*BEGIN NES SETUP
  pinMode(latch,OUTPUT);
  pinMode(clock,OUTPUT);
  pinMode(datin,INPUT);
  pinMode(ledpin, OUTPUT);
  
  digitalWrite(latch,HIGH);
  digitalWrite(clock,HIGH);
END NES SETUP*/

  // cmdMessenger.discard_LF_CR(); // Useful if your terminal appends CR/LF, and you wish to remove them
  cmdMessenger.print_LF_CR();   // Make output more readable whilst debugging in Arduino Serial Monitor

  // Attach default / generic callback methods
  cmdMessenger.attach(kARDUINO_READY, arduino_ready);
  cmdMessenger.attach(unknownCmd);

  // Attach my application's user-defined callback methods
  attach_callbacks(messengerCallbacks);

  arduino_ready();

  motor.setSpeed(20);  // 10 rpm   


  // blink
  //pinMode(13, OUTPUT);
}


// ------------------ M A I N ( ) --------------------------------------------

void loop() 
{  //DISABLED, MANUAL, PC

  cmdMessenger.feedinSerialData();
  //controllerRead();
  //manualControl();
}

  void manualControl()
  {//TODO use A/B while moving to accelerate
  
    //UP = 11110111
    //DOWN=11111011
    //Change sensitivity allowed outside of manual state
    if(controller_data==B11110111)
      manualSteps += 1;
    else if(controller_data==B11111011)
      if(manualSteps -1 > 0)
        manualSteps -= 1;
      //else min sensitivity reached
    
    if(trackingState != MANUAL)
    { //Just eat the buffer
      char buf[bufferSize] = { '\0' };
      cmdMessenger.copyString(buf, bufferSize);
      return;
    }
    
    int turnDirection = -1;
    
    motor.setSpeed(manualSpeed);
    
    //LEFT=11111101
    //RIGHT=11111110
    if(controller_data==B11111101) //Left
    {
      turnDirection = FORWARD; //Forward is arbitrary.
    }
    else if(controller_data==B11111110) //Right
    {
      turnDirection = BACKWARD; //Backward is arbitrary
    }
    else if(controller_data==B1111101) //Left+A
    {
       turnDirection = FORWARD;
       motor.setSpeed(manualSpeed*2);
    }
    else if(controller_data==B1111110) //Right+A
    {
       turnDirection = BACKWARD;
       motor.setSpeed(manualSpeed*2);
    }
    else if(controller_data==B10111101) //Left+B
    {
       turnDirection = FORWARD;
       motor.setSpeed(manualSpeed/2);
    }
    else if(controller_data==B10111110) //Right+B
    {
       turnDirection = BACKWARD;
       motor.setSpeed(manualSpeed/2);
    }
    else
      return; //Not valid direction

    if(manualSteps < MICRO_STEP_LIMIT)
    {
      motor.step(manualSteps, turnDirection, MICROSTEP);
    }
    else
    {
      motor.step(manualSteps, turnDirection, DOUBLE);
    }
    cmdMessenger.sendCmd(kACK,"ManMov");
  }

void setState()
{
    char buf[bufferSize] = { '\0' };
    cmdMessenger.copyString(buf, bufferSize);
    int state = atoi(buf);
    
    if(state == DISABLED)
      trackingState = DISABLED;
    else if(state == MANUAL)
      trackingState = MANUAL;
    else //Default to pc controlled
      trackingState = PC;
    Serial.println(trackingState);
}

//for REFERENCE:  
//UP = 11110111
//DOWN=11111011
//LEFT=11111101
//RIGHT=11111110
//SELECT=11011111
//START=11101111
//A=01111111
//B=10111111

  /* THIS READS DATA FROM NES CONTROLLER */
  void controllerRead() 
  {
    controller_data = 0;
    digitalWrite(latch,LOW);
    digitalWrite(clock,LOW);
    
    digitalWrite(latch,HIGH);
    delayMicroseconds(2);
    digitalWrite(latch,LOW);
    
    controller_data = digitalRead(datin);
    
    for (int i = 1; i <= 7; ++i) {
      digitalWrite(clock,HIGH);
      delayMicroseconds(2);
      controller_data = controller_data << 1;
      controller_data = controller_data + digitalRead(datin) ;
      delayMicroseconds(4);
      digitalWrite(clock,LOW);
      
    if(controller_data==B11111111)
    {
      return;
    }
    else if (controller_data==B11011111)//Select, drop state
    {
      if(trackingState == MANUAL)
      {
        trackingState = DISABLED;
        releaseMotor();
      }
      else if(trackingState == PC)
        trackingState = MANUAL;
      Serial.println(trackingState);
    }
    else if (controller_data==B11101111) //Start, step up state
    {
       if(trackingState == DISABLED)
         trackingState = MANUAL;
       else if(trackingState == MANUAL)
         trackingState = PC;
      Serial.println(trackingState);   
    }
  }

}


